<!DOCTYPE html PUBLIC "-//W3C//DTD XHTML 1.0 Transitional//EN" "http://www.w3.org/TR/xhtml1/DTD/xhtml1-transitional.dtd">
<html xmlns="http://www.w3.org/1999/xhtml">
<head>
<meta http-equiv="Content-Type" content="text/xhtml;charset=UTF-8"/>
<meta http-equiv="X-UA-Compatible" content="IE=9"/>
<meta name="generator" content="Doxygen 1.8.11"/>
<title>CL-CBS: sh_astar.cpp</title>
<link href="tabs.css" rel="stylesheet" type="text/css"/>
<script type="text/javascript" src="jquery.js"></script>
<script type="text/javascript" src="dynsections.js"></script>
<link href="search/search.css" rel="stylesheet" type="text/css"/>
<script type="text/javascript" src="search/searchdata.js"></script>
<script type="text/javascript" src="search/search.js"></script>
<script type="text/javascript">
  $(document).ready(function() { init_search(); });
</script>
<link href="doxygen.css" rel="stylesheet" type="text/css" />
</head>
<body>
<div id="top"><!-- do not remove this div, it is closed by doxygen! -->
<div id="titlearea">
<table cellspacing="0" cellpadding="0">
 <tbody>
 <tr style="height: 56px;">
  <td id="projectalign" style="padding-left: 0.5em;">
   <div id="projectname">CL-CBS
   </div>
  </td>
 </tr>
 </tbody>
</table>
</div>
<!-- end header part -->
<!-- Generated by Doxygen 1.8.11 -->
<script type="text/javascript">
var searchBox = new SearchBox("searchBox", "search",false,'Search');
</script>
  <div id="navrow1" class="tabs">
    <ul class="tablist">
      <li><a href="index.html"><span>Main&#160;Page</span></a></li>
      <li><a href="namespaces.html"><span>Namespaces</span></a></li>
      <li><a href="annotated.html"><span>Classes</span></a></li>
      <li><a href="files.html"><span>Files</span></a></li>
      <li><a href="examples.html"><span>Examples</span></a></li>
      <li>
        <div id="MSearchBox" class="MSearchBoxInactive">
        <span class="left">
          <img id="MSearchSelect" src="search/mag_sel.png"
               onmouseover="return searchBox.OnSearchSelectShow()"
               onmouseout="return searchBox.OnSearchSelectHide()"
               alt=""/>
          <input type="text" id="MSearchField" value="Search" accesskey="S"
               onfocus="searchBox.OnSearchFieldFocus(true)" 
               onblur="searchBox.OnSearchFieldFocus(false)" 
               onkeyup="searchBox.OnSearchFieldChange(event)"/>
          </span><span class="right">
            <a id="MSearchClose" href="javascript:searchBox.CloseResultsWindow()"><img id="MSearchCloseImg" border="0" src="search/close.png" alt=""/></a>
          </span>
        </div>
      </li>
    </ul>
  </div>
</div><!-- top -->
<!-- window showing the filter options -->
<div id="MSearchSelectWindow"
     onmouseover="return searchBox.OnSearchSelectShow()"
     onmouseout="return searchBox.OnSearchSelectHide()"
     onkeydown="return searchBox.OnSearchSelectKey(event)">
</div>

<!-- iframe showing the search results (closed by default) -->
<div id="MSearchResultsWindow">
<iframe src="javascript:void(0)" frameborder="0" 
        name="MSearchResults" id="MSearchResults">
</iframe>
</div>

<div class="header">
  <div class="headertitle">
<div class="title">sh_astar.cpp</div>  </div>
</div><!--header-->
<div class="contents">
<p>Implementation of Spatiotemporal Hybrid-State A* algorithm.</p>
<div class="fragment"><div class="line"></div><div class="line"><span class="preprocessor">#include &lt;math.h&gt;</span></div><div class="line"><span class="preprocessor">#include &lt;ompl/base/State.h&gt;</span></div><div class="line"><span class="preprocessor">#include &lt;ompl/base/spaces/DubinsStateSpace.h&gt;</span></div><div class="line"><span class="preprocessor">#include &lt;ompl/base/spaces/ReedsSheppStateSpace.h&gt;</span></div><div class="line"><span class="preprocessor">#include &lt;ompl/base/spaces/SE2StateSpace.h&gt;</span></div><div class="line"></div><div class="line"><span class="preprocessor">#include &lt;boost/functional/hash.hpp&gt;</span></div><div class="line"><span class="preprocessor">#include &lt;eigen3/Eigen/Dense&gt;</span></div><div class="line"><span class="preprocessor">#include &lt;fstream&gt;</span></div><div class="line"><span class="preprocessor">#include &lt;iostream&gt;</span></div><div class="line"><span class="keyword">typedef</span> ompl::base::SE2StateSpace::StateType <a name="a0"></a><a class="code" href="namespacelibMultiRobotPlanning.html#a844e866bb31d428ef897a8f22bda0cd0">OmplState</a>;</div><div class="line"></div><div class="line"><span class="preprocessor">#include &quot;<a class="code" href="hybrid__astar_8hpp.html">hybrid_astar.hpp</a>&quot;</span></div><div class="line"><span class="preprocessor">#include &quot;<a class="code" href="timer_8hpp.html">timer.hpp</a>&quot;</span></div><div class="line"></div><div class="line"><span class="keyword">using</span> <a name="_a1"></a><a class="code" href="classlibMultiRobotPlanning_1_1HybridAStar.html">libMultiRobotPlanning::HybridAStar</a>;</div><div class="line"><span class="keyword">using</span> <a name="_a2"></a><a class="code" href="structlibMultiRobotPlanning_1_1Neighbor.html">libMultiRobotPlanning::Neighbor</a>;</div><div class="line"><span class="keyword">using</span> <a name="_a3"></a><a class="code" href="structlibMultiRobotPlanning_1_1PlanResult.html">libMultiRobotPlanning::PlanResult</a>;</div><div class="line"><span class="keyword">using namespace </span><a class="code" href="namespacelibMultiRobotPlanning.html">libMultiRobotPlanning</a>;</div><div class="line"></div><div class="line"><span class="keyword">namespace </span><a class="code" href="namespaceConstants.html">Constants</a> {</div><div class="line"><span class="comment">// [m] --- The minimum turning radius of the vehicle</span></div><div class="line"><span class="keyword">static</span> <span class="keyword">const</span> <span class="keywordtype">float</span> r = 3;</div><div class="line"><span class="keyword">static</span> <span class="keyword">const</span> <span class="keywordtype">float</span> deltat = 6.75 / 180.0 * M_PI;</div><div class="line"><span class="comment">// [#] --- A movement cost penalty for turning (choosing non straight motion</span></div><div class="line"><span class="comment">// primitives)</span></div><div class="line"><span class="keyword">static</span> <span class="keyword">const</span> <span class="keywordtype">float</span> penaltyTurning = 1.3;</div><div class="line"><span class="comment">// [#] --- A movement cost penalty for reversing (choosing motion primitives &gt;</span></div><div class="line"><span class="comment">// 2)</span></div><div class="line"><span class="keyword">static</span> <span class="keyword">const</span> <span class="keywordtype">float</span> penaltyReversing = 2.0;</div><div class="line"><span class="comment">// [#] --- A movement cost penalty for change of direction (changing from</span></div><div class="line"><span class="comment">// primitives &lt; 3 to primitives &gt; 2)</span></div><div class="line"><span class="keyword">static</span> <span class="keyword">const</span> <span class="keywordtype">float</span> penaltyCOD = 2.0;</div><div class="line"><span class="comment">// map resolution</span></div><div class="line"><span class="keyword">static</span> <span class="keyword">const</span> <span class="keywordtype">float</span> mapResolution = 2.0;</div><div class="line"><span class="keyword">static</span> <span class="keyword">const</span> <span class="keywordtype">float</span> xyResolution = r * deltat;</div><div class="line"><span class="keyword">static</span> <span class="keyword">const</span> <span class="keywordtype">float</span> yawResolution = deltat;</div><div class="line"></div><div class="line"><span class="comment">// width of car</span></div><div class="line"><span class="keyword">static</span> <span class="keyword">const</span> <span class="keywordtype">float</span> carWidth = 2.0;</div><div class="line"><span class="comment">// distance from rear to vehicle front end</span></div><div class="line"><span class="keyword">static</span> <span class="keyword">const</span> <span class="keywordtype">float</span> LF = 2.0;</div><div class="line"><span class="comment">// distance from rear to vehicle back end</span></div><div class="line"><span class="keyword">static</span> <span class="keyword">const</span> <span class="keywordtype">float</span> LB = 1.0;</div><div class="line"><span class="comment">// obstacle default radius</span></div><div class="line"><span class="keyword">static</span> <span class="keyword">const</span> <span class="keywordtype">float</span> obsRadius = 1;</div><div class="line"></div><div class="line"><span class="comment">// R = 3, 6.75 DEG</span></div><div class="line"><span class="keyword">const</span> <span class="keywordtype">double</span> <a name="a4"></a><a class="code" href="namespaceConstants.html#a73c1390c57181cef88dd82781e2e6505">dx</a>[] = {r * deltat, r* sin(deltat),  r* sin(deltat),</div><div class="line">                     -r* deltat, -r* sin(deltat), -r* sin(deltat)};</div><div class="line"><span class="keyword">const</span> <span class="keywordtype">double</span> <a name="a5"></a><a class="code" href="namespaceConstants.html#a51c342708494b61085d51877cf6f1f28">dy</a>[] = {0, -r*(1 - cos(deltat)), r*(1 - cos(deltat)),</div><div class="line">                     0, -r*(1 - cos(deltat)), r*(1 - cos(deltat))};</div><div class="line"><span class="keyword">const</span> <span class="keywordtype">double</span> <a name="a6"></a><a class="code" href="namespaceConstants.html#a627bafd3befec8fad9e764920e9e56f4">dyaw</a>[] = {0, deltat, -deltat, 0, -deltat, deltat};</div><div class="line"></div><div class="line"><span class="keyword">static</span> <span class="keyword">inline</span> <span class="keywordtype">float</span> normalizeHeadingRad(<span class="keywordtype">float</span> t) {</div><div class="line">  <span class="keywordflow">if</span> (t &lt; 0) {</div><div class="line">    t = t - 2.f * M_PI * (int)(t / (2.f * M_PI));</div><div class="line">    <span class="keywordflow">return</span> 2.f * M_PI + t;</div><div class="line">  }</div><div class="line"></div><div class="line">  <span class="keywordflow">return</span> t - 2.f * M_PI * (int)(t / (2.f * M_PI));</div><div class="line">}</div><div class="line">}  <span class="comment">// namespace Constants</span></div><div class="line"></div><div class="line"><span class="keyword">struct </span>State {</div><div class="line">  State(<span class="keywordtype">double</span> x, <span class="keywordtype">double</span> y, <span class="keywordtype">double</span> yaw) : x(x), y(y), yaw(yaw) {}</div><div class="line">  State(<span class="keyword">const</span> State&amp;) = <span class="keywordflow">default</span>;</div><div class="line">  State(State&amp;&amp;) = <span class="keywordflow">default</span>;</div><div class="line">  State&amp; operator=(<span class="keyword">const</span> State&amp;) = <span class="keywordflow">default</span>;</div><div class="line">  State&amp; operator=(State&amp;&amp;) = <span class="keywordflow">default</span>;</div><div class="line"></div><div class="line">  <span class="keywordtype">bool</span> operator==(<span class="keyword">const</span> State&amp; other)<span class="keyword"> const </span>{</div><div class="line">    <span class="keywordflow">return</span> std::tie(x, y, yaw) == std::tie(other.x, other.y, other.yaw);</div><div class="line">  }</div><div class="line"></div><div class="line">  <span class="keyword">friend</span> std::ostream&amp; operator&lt;&lt;(std::ostream&amp; os, <span class="keyword">const</span> State&amp; s) {</div><div class="line">    <span class="keywordflow">return</span> os &lt;&lt; <span class="stringliteral">&quot;(&quot;</span> &lt;&lt; s.x &lt;&lt; <span class="stringliteral">&quot;,&quot;</span> &lt;&lt; s.y &lt;&lt; <span class="stringliteral">&quot;:&quot;</span> &lt;&lt; s.yaw &lt;&lt; <span class="stringliteral">&quot;)&quot;</span>;</div><div class="line">  }</div><div class="line"></div><div class="line">  <span class="keywordtype">double</span> x;</div><div class="line">  <span class="keywordtype">double</span> y;</div><div class="line">  <span class="keywordtype">double</span> yaw;</div><div class="line">};</div><div class="line"></div><div class="line"><span class="keyword">namespace </span>std {</div><div class="line"><span class="keyword">template</span> &lt;&gt;</div><div class="line"><span class="keyword">struct </span>hash&lt;State&gt; {</div><div class="line">  <span class="keywordtype">size_t</span> operator()(<span class="keyword">const</span> State&amp; s)<span class="keyword"> const </span>{</div><div class="line">    <span class="keywordtype">size_t</span> seed = 0;</div><div class="line">    boost::hash_combine(seed, s.x);</div><div class="line">    boost::hash_combine(seed, s.y);</div><div class="line">    boost::hash_combine(seed, s.yaw);</div><div class="line">    <span class="keywordflow">return</span> seed;</div><div class="line">  }</div><div class="line">};</div><div class="line">}  <span class="comment">// namespace std</span></div><div class="line"></div><div class="line"><span class="keyword">using</span> Action = int;  <span class="comment">// Action &lt; 6</span></div><div class="line"></div><div class="line"><span class="keyword">class </span><a name="_a7"></a><a class="code" href="classlibMultiRobotPlanning_1_1Environment.html">Environment</a> {</div><div class="line"> <span class="keyword">public</span>:</div><div class="line">  <a class="code" href="classlibMultiRobotPlanning_1_1Environment.html">Environment</a>(<span class="keywordtype">size_t</span> maxx, <span class="keywordtype">size_t</span> maxy, std::unordered_set&lt;State&gt; obstacles,</div><div class="line">              State goal)</div><div class="line">      : m_obstacles(std::move(obstacles)),</div><div class="line">        m_goal(goal)  <span class="comment">// NOLINT</span></div><div class="line">  {</div><div class="line">    m_dimx = (int)maxx / Constants::mapResolution;</div><div class="line">    m_dimy = (int)maxy / Constants::mapResolution;</div><div class="line">    <span class="comment">// std::cout &lt;&lt; &quot;env build &quot; &lt;&lt; m_dimx &lt;&lt; &quot; &quot; &lt;&lt; m_dimy &lt;&lt; &quot; &quot;</span></div><div class="line">    <span class="comment">//           &lt;&lt; m_obstacles.size() &lt;&lt; std::endl;</span></div><div class="line">    holonomic_cost_map = std::vector&lt;std::vector&lt;double&gt;&gt;(</div><div class="line">        m_dimx, std::vector&lt;double&gt;(m_dimy, 0));</div><div class="line">    m_goal = State(goal.x, goal.y, Constants::normalizeHeadingRad(goal.yaw));</div><div class="line">    updateCostmap();</div><div class="line">  }</div><div class="line"></div><div class="line">  <span class="keyword">struct </span>compare_node {</div><div class="line">    <span class="keywordtype">bool</span> operator()(<span class="keyword">const</span> std::pair&lt;State, double&gt;&amp; n1,</div><div class="line">                    <span class="keyword">const</span> std::pair&lt;State, double&gt;&amp; n2)<span class="keyword"> const </span>{</div><div class="line">      <span class="keywordflow">return</span> (n1.second &gt; n2.second);</div><div class="line">    }</div><div class="line">  };</div><div class="line"></div><div class="line">  uint64_t calcIndex(<span class="keyword">const</span> State&amp; s) {</div><div class="line">    <span class="keywordflow">return</span> (uint64_t)(Constants::normalizeHeadingRad(s.yaw) /</div><div class="line">                      Constants::yawResolution) *</div><div class="line">               (m_dimx * Constants::mapResolution / Constants::xyResolution) *</div><div class="line">               (m_dimy * Constants::mapResolution / Constants::xyResolution) +</div><div class="line">           (uint64_t)(s.y / Constants::xyResolution) *</div><div class="line">               (m_dimx * Constants::mapResolution / Constants::xyResolution) +</div><div class="line">           (uint64_t)(s.x / Constants::xyResolution);</div><div class="line">  }</div><div class="line"></div><div class="line">  <span class="keywordtype">double</span> admissibleHeuristic(<span class="keyword">const</span> State&amp; s) {</div><div class="line">    <span class="comment">// non-holonomic-without-obstacles heuristic: use a Reeds-Shepp</span></div><div class="line">    ompl::base::ReedsSheppStateSpace reedsSheppPath(Constants::r);</div><div class="line">    <a class="code" href="namespacelibMultiRobotPlanning.html#a844e866bb31d428ef897a8f22bda0cd0">OmplState</a>* rsStart = (<a class="code" href="namespacelibMultiRobotPlanning.html#a844e866bb31d428ef897a8f22bda0cd0">OmplState</a>*)reedsSheppPath.allocState();</div><div class="line">    <a class="code" href="namespacelibMultiRobotPlanning.html#a844e866bb31d428ef897a8f22bda0cd0">OmplState</a>* rsEnd = (<a class="code" href="namespacelibMultiRobotPlanning.html#a844e866bb31d428ef897a8f22bda0cd0">OmplState</a>*)reedsSheppPath.allocState();</div><div class="line">    rsStart-&gt;setXY(s.x, s.y);</div><div class="line">    rsStart-&gt;setYaw(s.yaw);</div><div class="line">    rsEnd-&gt;setXY(m_goal.x, m_goal.y);</div><div class="line">    rsEnd-&gt;setYaw(m_goal.yaw);</div><div class="line">    <span class="keywordtype">double</span> reedsSheppCost = reedsSheppPath.distance(rsStart, rsEnd);</div><div class="line">    <span class="comment">// std::cout &lt;&lt; &quot;ReedsShepps cost:&quot; &lt;&lt; reedsSheppCost &lt;&lt; std::endl;</span></div><div class="line">    <span class="comment">// Euclidean distance</span></div><div class="line">    <span class="keywordtype">double</span> euclideanCost =</div><div class="line">        sqrt(pow(m_goal.x - s.x, 2) + pow(m_goal.y - s.y, 2));</div><div class="line">    <span class="comment">// std::cout &lt;&lt; &quot;Euclidean cost:&quot; &lt;&lt; euclideanCost &lt;&lt; std::endl;</span></div><div class="line">    <span class="comment">// holonomic-with-obstacles heuristic</span></div><div class="line">    <span class="keywordtype">double</span> twoDoffset =</div><div class="line">        sqrt(pow((s.x - (<span class="keywordtype">int</span>)s.x) - (m_goal.x - (<span class="keywordtype">int</span>)m_goal.x), 2) +</div><div class="line">             pow((s.y - (<span class="keywordtype">int</span>)s.y) - (m_goal.y - (<span class="keywordtype">int</span>)m_goal.y), 2));</div><div class="line">    <span class="keywordtype">double</span> twoDCost = holonomic_cost_map[(int)s.x / Constants::mapResolution]</div><div class="line">                                        [(<span class="keywordtype">int</span>)s.y / Constants::mapResolution] -</div><div class="line">                      twoDoffset;</div><div class="line">    <span class="comment">// std::cout &lt;&lt; &quot;holonomic cost:&quot; &lt;&lt; twoDCost &lt;&lt; std::endl;</span></div><div class="line"></div><div class="line">    <span class="keywordflow">return</span> std::max({reedsSheppCost, euclideanCost, twoDCost});</div><div class="line">  }</div><div class="line"></div><div class="line">  <span class="keywordtype">bool</span> isSolution(</div><div class="line">      <span class="keyword">const</span> State&amp; state, <span class="keywordtype">double</span> gscore,</div><div class="line">      std::unordered_map&lt;State, std::tuple&lt;State, Action, double, double&gt;,</div><div class="line">                         std::hash&lt;State&gt;&gt;&amp; _camefrom) {</div><div class="line">    <span class="keywordtype">double</span> goal_distance =</div><div class="line">        sqrt(pow(state.x - m_goal.x, 2) + pow(state.y - m_goal.y, 2));</div><div class="line">    <span class="keywordflow">if</span> (goal_distance &gt; 2 * (Constants::LB + Constants::LF)) <span class="keywordflow">return</span> <span class="keyword">false</span>;</div><div class="line"></div><div class="line">    ompl::base::ReedsSheppStateSpace reedsSheppSpace(Constants::r);</div><div class="line">    <a class="code" href="namespacelibMultiRobotPlanning.html#a844e866bb31d428ef897a8f22bda0cd0">OmplState</a>* rsStart = (<a class="code" href="namespacelibMultiRobotPlanning.html#a844e866bb31d428ef897a8f22bda0cd0">OmplState</a>*)reedsSheppSpace.allocState();</div><div class="line">    <a class="code" href="namespacelibMultiRobotPlanning.html#a844e866bb31d428ef897a8f22bda0cd0">OmplState</a>* rsEnd = (<a class="code" href="namespacelibMultiRobotPlanning.html#a844e866bb31d428ef897a8f22bda0cd0">OmplState</a>*)reedsSheppSpace.allocState();</div><div class="line">    rsStart-&gt;setXY(state.x, state.y);</div><div class="line">    rsStart-&gt;setYaw(-state.yaw);</div><div class="line">    rsEnd-&gt;setXY(m_goal.x, m_goal.y);</div><div class="line">    rsEnd-&gt;setYaw(-m_goal.yaw);</div><div class="line">    ompl::base::ReedsSheppStateSpace::ReedsSheppPath reedsShepppath =</div><div class="line">        reedsSheppSpace.reedsShepp(rsStart, rsEnd);</div><div class="line"></div><div class="line">    std::vector&lt;State&gt; path;</div><div class="line">    std::unordered_map&lt;State, std::tuple&lt;State, Action, double, double&gt;,</div><div class="line">                       std::hash&lt;State&gt;&gt;</div><div class="line">        cameFrom;</div><div class="line">    cameFrom.clear();</div><div class="line">    path.emplace_back(state);</div><div class="line">    <span class="keywordflow">for</span> (<span class="keyword">auto</span> pathidx = 0; pathidx &lt; 5; pathidx++) {</div><div class="line">      <span class="keywordflow">if</span> (fabs(reedsShepppath.length_[pathidx]) &lt; 1e-6) <span class="keywordflow">continue</span>;</div><div class="line">      <span class="keywordtype">double</span> deltat, <a class="code" href="namespaceConstants.html#a73c1390c57181cef88dd82781e2e6505">dx</a>, act, cost;</div><div class="line">      <span class="keywordflow">switch</span> (reedsShepppath.type_[pathidx]) {</div><div class="line">        <span class="keywordflow">case</span> 0:  <span class="comment">// RS_NOP</span></div><div class="line">          <span class="keywordflow">continue</span>;</div><div class="line">          <span class="keywordflow">break</span>;</div><div class="line">        <span class="keywordflow">case</span> 1:  <span class="comment">// RS_LEFT</span></div><div class="line">          deltat = -reedsShepppath.length_[pathidx];</div><div class="line">          dx = Constants::r * sin(-deltat);</div><div class="line">          <span class="comment">// dy = Constants::r * (1 - cos(-deltat));</span></div><div class="line">          act = 2;</div><div class="line">          cost = reedsShepppath.length_[pathidx] * Constants::r *</div><div class="line">                 Constants::penaltyTurning;</div><div class="line">          <span class="keywordflow">break</span>;</div><div class="line">        <span class="keywordflow">case</span> 2:  <span class="comment">// RS_STRAIGHT</span></div><div class="line">          deltat = 0;</div><div class="line">          dx = reedsShepppath.length_[pathidx] * Constants::r;</div><div class="line">          <span class="comment">// dy = 0;</span></div><div class="line">          act = 0;</div><div class="line">          cost = <a class="code" href="namespaceConstants.html#a73c1390c57181cef88dd82781e2e6505">dx</a>;</div><div class="line">          <span class="keywordflow">break</span>;</div><div class="line">        <span class="keywordflow">case</span> 3:  <span class="comment">// RS_RIGHT</span></div><div class="line">          deltat = reedsShepppath.length_[pathidx];</div><div class="line">          dx = Constants::r * sin(deltat);</div><div class="line">          <span class="comment">// dy = -Constants::r * (1 - cos(deltat));</span></div><div class="line">          act = 1;</div><div class="line">          cost = reedsShepppath.length_[pathidx] * Constants::r *</div><div class="line">                 Constants::penaltyTurning;</div><div class="line">          <span class="keywordflow">break</span>;</div><div class="line">        <span class="keywordflow">default</span>:</div><div class="line">          std::cout &lt;&lt; <span class="stringliteral">&quot;\033[1m\033[31m&quot;</span></div><div class="line">                    &lt;&lt; <span class="stringliteral">&quot;Warning: Receive unknown ReedsSheppPath type&quot;</span></div><div class="line">                    &lt;&lt; <span class="stringliteral">&quot;\033[0m\n&quot;</span>;</div><div class="line">          <span class="keywordflow">break</span>;</div><div class="line">      }</div><div class="line">      <span class="keywordflow">if</span> (cost &lt; 0) {</div><div class="line">        cost = -cost * Constants::penaltyReversing;</div><div class="line">        act = act + 3;</div><div class="line">      }</div><div class="line">      State s = path.back();</div><div class="line">      std::vector&lt;std::pair&lt;State, double&gt;&gt; next_path =</div><div class="line">          generatePath(s, act, deltat, dx);</div><div class="line">      <span class="comment">// State next_s(s.x + dx * cos(-s.yaw) - dy * sin(-s.yaw),</span></div><div class="line">      <span class="comment">//              s.y + dx * sin(-s.yaw) + dy * cos(-s.yaw),</span></div><div class="line">      <span class="comment">//              Constants::normalizeHeadingRad(s.yaw + deltat));</span></div><div class="line">      <span class="keywordflow">for</span> (<span class="keyword">auto</span> iter = next_path.begin(); iter != next_path.end(); iter++) {</div><div class="line">        State next_s = iter-&gt;first;</div><div class="line">        <span class="keywordflow">if</span> (!stateValid(next_s))</div><div class="line">          <span class="keywordflow">return</span> <span class="keyword">false</span>;</div><div class="line">        <span class="keywordflow">else</span> {</div><div class="line">          gscore += iter-&gt;second;</div><div class="line">          <span class="keywordflow">if</span> (!(next_s == path.back())) {</div><div class="line">            cameFrom.insert(std::make_pair&lt;&gt;(</div><div class="line">                next_s,</div><div class="line">                std::make_tuple&lt;&gt;(path.back(), act, iter-&gt;second, gscore)));</div><div class="line">          }</div><div class="line">          path.emplace_back(next_s);</div><div class="line">        }</div><div class="line">      }</div><div class="line">    }</div><div class="line"></div><div class="line">    m_goal = path.back();</div><div class="line">    <span class="comment">// auto iter = cameFrom.find(getGoal());</span></div><div class="line">    <span class="comment">// do {</span></div><div class="line">    <span class="comment">//   std::cout &lt;&lt; &quot; From &quot; &lt;&lt; std::get&lt;0&gt;(iter-&gt;second)</span></div><div class="line">    <span class="comment">//             &lt;&lt; &quot; to Node:&quot; &lt;&lt; iter-&gt;first</span></div><div class="line">    <span class="comment">//             &lt;&lt; &quot; with ACTION: &quot; &lt;&lt; std::get&lt;1&gt;(iter-&gt;second) &lt;&lt; &quot; cost &quot;</span></div><div class="line">    <span class="comment">//             &lt;&lt; std::get&lt;2&gt;(iter-&gt;second) &lt;&lt; &quot; g_score &quot;</span></div><div class="line">    <span class="comment">//             &lt;&lt; std::get&lt;3&gt;(iter-&gt;second) &lt;&lt; std::endl;</span></div><div class="line">    <span class="comment">//   iter = cameFrom.find(std::get&lt;0&gt;(iter-&gt;second));</span></div><div class="line">    <span class="comment">// } while (calcIndex(std::get&lt;0&gt;(iter-&gt;second)) != calcIndex(state));</span></div><div class="line">    <span class="comment">// std::cout &lt;&lt; &quot; From &quot; &lt;&lt; std::get&lt;0&gt;(iter-&gt;second)</span></div><div class="line">    <span class="comment">//           &lt;&lt; &quot; to Node:&quot; &lt;&lt; iter-&gt;first</span></div><div class="line">    <span class="comment">//           &lt;&lt; &quot; with ACTION: &quot; &lt;&lt; std::get&lt;1&gt;(iter-&gt;second) &lt;&lt; &quot; cost &quot;</span></div><div class="line">    <span class="comment">//           &lt;&lt; std::get&lt;2&gt;(iter-&gt;second) &lt;&lt; &quot; g_score &quot;</span></div><div class="line">    <span class="comment">//           &lt;&lt; std::get&lt;3&gt;(iter-&gt;second) &lt;&lt; std::endl;</span></div><div class="line"></div><div class="line">    _camefrom.insert(cameFrom.begin(), cameFrom.end());</div><div class="line">    <span class="keywordflow">return</span> <span class="keyword">true</span>;</div><div class="line">  }</div><div class="line"></div><div class="line">  <span class="keywordtype">void</span> getNeighbors(<span class="keyword">const</span> State&amp; s, Action action,</div><div class="line">                    std::vector&lt;<a class="code" href="structlibMultiRobotPlanning_1_1Neighbor.html">Neighbor&lt;State, Action, double&gt;</a>&gt;&amp; neighbors) {</div><div class="line">    neighbors.clear();</div><div class="line">    <span class="keywordflow">for</span> (Action act = 0; act &lt; 6; act++) {  <span class="comment">// has 6 directions for Reeds-Shepp</span></div><div class="line">      <span class="keywordtype">double</span> xSucc, ySucc, yawSucc;</div><div class="line">      <span class="keywordtype">double</span> g = <a class="code" href="namespaceConstants.html#a73c1390c57181cef88dd82781e2e6505">Constants::dx</a>[0];</div><div class="line">      xSucc = s.x + <a class="code" href="namespaceConstants.html#a73c1390c57181cef88dd82781e2e6505">Constants::dx</a>[act] * cos(-s.yaw) -</div><div class="line">              <a class="code" href="namespaceConstants.html#a51c342708494b61085d51877cf6f1f28">Constants::dy</a>[act] * sin(-s.yaw);</div><div class="line">      ySucc = s.y + <a class="code" href="namespaceConstants.html#a73c1390c57181cef88dd82781e2e6505">Constants::dx</a>[act] * sin(-s.yaw) +</div><div class="line">              <a class="code" href="namespaceConstants.html#a51c342708494b61085d51877cf6f1f28">Constants::dy</a>[act] * cos(-s.yaw);</div><div class="line">      yawSucc = Constants::normalizeHeadingRad(s.yaw + <a class="code" href="namespaceConstants.html#a627bafd3befec8fad9e764920e9e56f4">Constants::dyaw</a>[act]);</div><div class="line">      <span class="keywordflow">if</span> (act != action) {  <span class="comment">// penalize turning</span></div><div class="line">        g = g * Constants::penaltyTurning;</div><div class="line">        <span class="keywordflow">if</span> (act &gt;= 3)  <span class="comment">// penalize change of direction</span></div><div class="line">          g = g * Constants::penaltyCOD;</div><div class="line">      }</div><div class="line">      <span class="keywordflow">if</span> (act &gt; 3) {  <span class="comment">// backwards</span></div><div class="line">        g = g * Constants::penaltyReversing;</div><div class="line">      }</div><div class="line">      State tempState(xSucc, ySucc, yawSucc);</div><div class="line">      <span class="keywordflow">if</span> (stateValid(tempState)) {</div><div class="line">        neighbors.emplace_back(</div><div class="line">            <a class="code" href="structlibMultiRobotPlanning_1_1Neighbor.html">Neighbor&lt;State, Action, double&gt;</a>(tempState, act, g));</div><div class="line">      }</div><div class="line">    }</div><div class="line">  }</div><div class="line"></div><div class="line">  <span class="keywordtype">void</span> onExpandNode(<span class="keyword">const</span> State&amp; s, <span class="keywordtype">int</span> <span class="comment">/*fScore*/</span>, <span class="keywordtype">int</span> <span class="comment">/*gScore*/</span>) {</div><div class="line">    Ecount++;</div><div class="line">    <span class="comment">// std::cout &lt;&lt; &quot;Expand &quot; &lt;&lt; Ecount &lt;&lt; &quot; new Node:&quot; &lt;&lt; s &lt;&lt; std::endl;</span></div><div class="line">  }</div><div class="line"></div><div class="line">  <span class="keywordtype">void</span> onDiscover(<span class="keyword">const</span> State&amp; s, <span class="keywordtype">double</span> fScore, <span class="keywordtype">double</span> gScore) {</div><div class="line">    Dcount++;</div><div class="line">    <span class="comment">// std::cout &lt;&lt; &quot;Discover &quot; &lt;&lt; Dcount &lt;&lt; &quot;  Node:&quot; &lt;&lt; s &lt;&lt; &quot; f:&quot; &lt;&lt; fScore</span></div><div class="line">    <span class="comment">//           &lt;&lt; &quot; g:&quot; &lt;&lt; gScore &lt;&lt; std::endl;</span></div><div class="line">  }</div><div class="line"></div><div class="line"> <span class="keyword">public</span>:</div><div class="line">  State getGoal() { <span class="keywordflow">return</span> m_goal; }</div><div class="line">  <span class="keywordtype">int</span> Ecount = 0;</div><div class="line">  <span class="keywordtype">int</span> Dcount = 0;</div><div class="line"></div><div class="line"> <span class="keyword">private</span>:</div><div class="line">  <span class="keywordtype">bool</span> stateValid(<span class="keyword">const</span> State&amp; s) {</div><div class="line">    <span class="keywordtype">double</span> x_ind = s.x / Constants::mapResolution;</div><div class="line">    <span class="keywordtype">double</span> y_ind = s.y / Constants::mapResolution;</div><div class="line">    <span class="keywordflow">if</span> (x_ind &lt; 0 || x_ind &gt;= m_dimx || y_ind &lt; 0 || y_ind &gt;= m_dimy)</div><div class="line">      <span class="keywordflow">return</span> <span class="keyword">false</span>;</div><div class="line"></div><div class="line">    Eigen::Matrix2f rot;</div><div class="line">    rot &lt;&lt; cos(-s.yaw), -sin(-s.yaw), sin(-s.yaw), cos(-s.yaw);</div><div class="line">    <span class="keywordflow">for</span> (<span class="keyword">auto</span> it = m_obstacles.begin(); it != m_obstacles.end(); it++) {</div><div class="line">      Eigen::Matrix&lt;float, 1, 2&gt; obs;</div><div class="line">      obs &lt;&lt; it-&gt;x - s.x, it-&gt;y - s.y;</div><div class="line">      <span class="keyword">auto</span> rotated_obs = obs * rot;</div><div class="line">      <span class="keywordflow">if</span> (rotated_obs(0) &gt; -Constants::LB - Constants::obsRadius &amp;&amp;</div><div class="line">          rotated_obs(0) &lt; Constants::LF + Constants::obsRadius &amp;&amp;</div><div class="line">          rotated_obs(1) &gt; -Constants::carWidth / 2.0 - Constants::obsRadius &amp;&amp;</div><div class="line">          rotated_obs(1) &lt; Constants::carWidth / 2.0 + Constants::obsRadius)</div><div class="line">        <span class="keywordflow">return</span> <span class="keyword">false</span>;</div><div class="line">    }</div><div class="line">    <span class="keywordflow">return</span> <span class="keyword">true</span>;</div><div class="line">    <span class="comment">// Eigen::Matrix2f rot;</span></div><div class="line">    <span class="comment">// double yaw = M_PI / 2;</span></div><div class="line">    <span class="comment">// rot &lt;&lt; cos(yaw), -sin(yaw), sin(yaw), cos(yaw);</span></div><div class="line">    <span class="comment">// Eigen::Matrix&lt;float, 1, 2&gt; temp;</span></div><div class="line">    <span class="comment">// temp &lt;&lt; 1, 2;</span></div><div class="line">    <span class="comment">// auto ro = temp * rot;</span></div><div class="line">    <span class="comment">// std::cout &lt;&lt; ro(0) &lt;&lt; ro(1) &lt;&lt; std::endl;</span></div><div class="line">  }</div><div class="line"></div><div class="line">  <span class="keywordtype">void</span> updateCostmap() {</div><div class="line">    boost::heap::fibonacci_heap&lt;std::pair&lt;State, double&gt;,</div><div class="line">                                boost::heap::compare&lt;compare_node&gt;&gt;</div><div class="line">        heap;</div><div class="line">    heap.clear();</div><div class="line"></div><div class="line">    std::set&lt;std::pair&lt;int, int&gt;&gt; temp_obs_set;</div><div class="line">    <span class="keywordflow">for</span> (<span class="keyword">auto</span> it = m_obstacles.begin(); it != m_obstacles.end(); it++) {</div><div class="line">      temp_obs_set.insert(</div><div class="line">          std::make_pair((<span class="keywordtype">int</span>)it-&gt;x / Constants::mapResolution,</div><div class="line">                         (<span class="keywordtype">int</span>)it-&gt;y / Constants::mapResolution));</div><div class="line">    }</div><div class="line"></div><div class="line">    <span class="keywordtype">int</span> goal_x = (int)m_goal.x / Constants::mapResolution;</div><div class="line">    <span class="keywordtype">int</span> goal_y = (<span class="keywordtype">int</span>)m_goal.y / Constants::mapResolution;</div><div class="line">    heap.push(std::make_pair(State(goal_x, goal_y, 0), 0));</div><div class="line"></div><div class="line">    <span class="keywordflow">while</span> (!heap.empty()) {</div><div class="line">      std::pair&lt;State, double&gt; node = heap.top();</div><div class="line">      heap.pop();</div><div class="line"></div><div class="line">      <span class="keywordtype">int</span> x = node.first.x;</div><div class="line">      <span class="keywordtype">int</span> y = node.first.y;</div><div class="line">      <span class="keywordflow">for</span> (<span class="keywordtype">int</span> <a class="code" href="namespaceConstants.html#a73c1390c57181cef88dd82781e2e6505">dx</a> = -1; <a class="code" href="namespaceConstants.html#a73c1390c57181cef88dd82781e2e6505">dx</a> &lt;= 1; <a class="code" href="namespaceConstants.html#a73c1390c57181cef88dd82781e2e6505">dx</a>++)</div><div class="line">        <span class="keywordflow">for</span> (<span class="keywordtype">int</span> <a class="code" href="namespaceConstants.html#a51c342708494b61085d51877cf6f1f28">dy</a> = -1; <a class="code" href="namespaceConstants.html#a51c342708494b61085d51877cf6f1f28">dy</a> &lt;= 1; <a class="code" href="namespaceConstants.html#a51c342708494b61085d51877cf6f1f28">dy</a>++) {</div><div class="line">          <span class="keywordflow">if</span> (<a class="code" href="namespaceConstants.html#a73c1390c57181cef88dd82781e2e6505">dx</a> == 0 &amp;&amp; <a class="code" href="namespaceConstants.html#a51c342708494b61085d51877cf6f1f28">dy</a> == 0) <span class="keywordflow">continue</span>;</div><div class="line">          <span class="keywordtype">int</span> new_x = x + <a class="code" href="namespaceConstants.html#a73c1390c57181cef88dd82781e2e6505">dx</a>;</div><div class="line">          <span class="keywordtype">int</span> new_y = y + <a class="code" href="namespaceConstants.html#a51c342708494b61085d51877cf6f1f28">dy</a>;</div><div class="line">          <span class="keywordflow">if</span> (new_x == goal_x &amp;&amp; new_y == goal_y) <span class="keywordflow">continue</span>;</div><div class="line">          <span class="keywordflow">if</span> (new_x &gt;= 0 &amp;&amp; new_x &lt; m_dimx &amp;&amp; new_y &gt;= 0 &amp;&amp; new_y &lt; m_dimy &amp;&amp;</div><div class="line">              holonomic_cost_map[new_x][new_y] == 0 &amp;&amp;</div><div class="line">              temp_obs_set.find(std::make_pair(new_x, new_y)) ==</div><div class="line">                  temp_obs_set.end()) {</div><div class="line">            holonomic_cost_map[new_x][new_y] =</div><div class="line">                holonomic_cost_map[x][y] +</div><div class="line">                sqrt(pow(dx * Constants::mapResolution, 2) +</div><div class="line">                     pow(dy * Constants::mapResolution, 2));</div><div class="line">            heap.push(std::make_pair(State(new_x, new_y, 0),</div><div class="line">                                     holonomic_cost_map[new_x][new_y]));</div><div class="line">          }</div><div class="line">        }</div><div class="line">    }</div><div class="line"></div><div class="line">    <span class="comment">// for (size_t i = 0; i &lt; m_dimx; i++) {</span></div><div class="line">    <span class="comment">//   for (size_t j = 0; j &lt; m_dimy; j++)</span></div><div class="line">    <span class="comment">//     std::cout &lt;&lt; holonomic_cost_map[i][j] &lt;&lt; &quot;\t&quot;;</span></div><div class="line">    <span class="comment">//   std::cout &lt;&lt; std::endl;</span></div><div class="line">    <span class="comment">// }</span></div><div class="line">  }</div><div class="line"></div><div class="line">  std::vector&lt;std::pair&lt;State, double&gt;&gt; generatePath(State startState, <span class="keywordtype">int</span> act,</div><div class="line">                                                     <span class="keywordtype">double</span> deltaSteer,</div><div class="line">                                                     <span class="keywordtype">double</span> deltaLength) {</div><div class="line">    std::vector&lt;std::pair&lt;State, double&gt;&gt; result;</div><div class="line">    <span class="keywordtype">double</span> xSucc, ySucc, yawSucc, <a class="code" href="namespaceConstants.html#a73c1390c57181cef88dd82781e2e6505">dx</a>, <a class="code" href="namespaceConstants.html#a51c342708494b61085d51877cf6f1f28">dy</a>, <a class="code" href="namespaceConstants.html#a627bafd3befec8fad9e764920e9e56f4">dyaw</a>, ratio;</div><div class="line">    result.emplace_back(std::make_pair&lt;&gt;(startState, 0));</div><div class="line">    <span class="keywordflow">if</span> (act == 0 || act == 3) {</div><div class="line">      <span class="keywordflow">for</span> (<span class="keywordtype">size_t</span> i = 0; i &lt; (size_t)(deltaLength / <a class="code" href="namespaceConstants.html#a73c1390c57181cef88dd82781e2e6505">Constants::dx</a>[act]); i++) {</div><div class="line">        State s = result.back().first;</div><div class="line">        xSucc = s.x + <a class="code" href="namespaceConstants.html#a73c1390c57181cef88dd82781e2e6505">Constants::dx</a>[act] * cos(-s.yaw) -</div><div class="line">                <a class="code" href="namespaceConstants.html#a51c342708494b61085d51877cf6f1f28">Constants::dy</a>[act] * sin(-s.yaw);</div><div class="line">        ySucc = s.y + <a class="code" href="namespaceConstants.html#a73c1390c57181cef88dd82781e2e6505">Constants::dx</a>[act] * sin(-s.yaw) +</div><div class="line">                <a class="code" href="namespaceConstants.html#a51c342708494b61085d51877cf6f1f28">Constants::dy</a>[act] * cos(-s.yaw);</div><div class="line">        yawSucc = Constants::normalizeHeadingRad(s.yaw + <a class="code" href="namespaceConstants.html#a627bafd3befec8fad9e764920e9e56f4">Constants::dyaw</a>[act]);</div><div class="line">        result.emplace_back(</div><div class="line">            std::make_pair&lt;&gt;(State(xSucc, ySucc, yawSucc), <a class="code" href="namespaceConstants.html#a73c1390c57181cef88dd82781e2e6505">Constants::dx</a>[0]));</div><div class="line">      }</div><div class="line">      ratio = (deltaLength -</div><div class="line">               (int)(deltaLength / <a class="code" href="namespaceConstants.html#a73c1390c57181cef88dd82781e2e6505">Constants::dx</a>[act]) * <a class="code" href="namespaceConstants.html#a73c1390c57181cef88dd82781e2e6505">Constants::dx</a>[act]) /</div><div class="line">              <a class="code" href="namespaceConstants.html#a73c1390c57181cef88dd82781e2e6505">Constants::dx</a>[act];</div><div class="line">      dyaw = 0;</div><div class="line">      dx = ratio * <a class="code" href="namespaceConstants.html#a73c1390c57181cef88dd82781e2e6505">Constants::dx</a>[act];</div><div class="line">      dy = 0;</div><div class="line">    } <span class="keywordflow">else</span> {</div><div class="line">      <span class="keywordflow">for</span> (<span class="keywordtype">size_t</span> i = 0; i &lt; (size_t)(deltaSteer / <a class="code" href="namespaceConstants.html#a627bafd3befec8fad9e764920e9e56f4">Constants::dyaw</a>[act]); i++) {</div><div class="line">        State s = result.back().first;</div><div class="line">        xSucc = s.x + <a class="code" href="namespaceConstants.html#a73c1390c57181cef88dd82781e2e6505">Constants::dx</a>[act] * cos(-s.yaw) -</div><div class="line">                <a class="code" href="namespaceConstants.html#a51c342708494b61085d51877cf6f1f28">Constants::dy</a>[act] * sin(-s.yaw);</div><div class="line">        ySucc = s.y + <a class="code" href="namespaceConstants.html#a73c1390c57181cef88dd82781e2e6505">Constants::dx</a>[act] * sin(-s.yaw) +</div><div class="line">                <a class="code" href="namespaceConstants.html#a51c342708494b61085d51877cf6f1f28">Constants::dy</a>[act] * cos(-s.yaw);</div><div class="line">        yawSucc = Constants::normalizeHeadingRad(s.yaw + <a class="code" href="namespaceConstants.html#a627bafd3befec8fad9e764920e9e56f4">Constants::dyaw</a>[act]);</div><div class="line">        result.emplace_back(</div><div class="line">            std::make_pair&lt;&gt;(State(xSucc, ySucc, yawSucc),</div><div class="line">                             <a class="code" href="namespaceConstants.html#a73c1390c57181cef88dd82781e2e6505">Constants::dx</a>[0] * Constants::penaltyTurning));</div><div class="line">      }</div><div class="line">      ratio =</div><div class="line">          (deltaSteer -</div><div class="line">           (int)(deltaSteer / <a class="code" href="namespaceConstants.html#a627bafd3befec8fad9e764920e9e56f4">Constants::dyaw</a>[act]) * <a class="code" href="namespaceConstants.html#a627bafd3befec8fad9e764920e9e56f4">Constants::dyaw</a>[act]) /</div><div class="line">          <a class="code" href="namespaceConstants.html#a627bafd3befec8fad9e764920e9e56f4">Constants::dyaw</a>[act];</div><div class="line">      dyaw = ratio * <a class="code" href="namespaceConstants.html#a627bafd3befec8fad9e764920e9e56f4">Constants::dyaw</a>[act];</div><div class="line">      dx = Constants::r * sin(dyaw);</div><div class="line">      dy = -Constants::r * (1 - cos(dyaw));</div><div class="line">      <span class="keywordflow">if</span> (act == 2 || act == 5) {</div><div class="line">        dx = -<a class="code" href="namespaceConstants.html#a73c1390c57181cef88dd82781e2e6505">dx</a>;</div><div class="line">        dy = -<a class="code" href="namespaceConstants.html#a51c342708494b61085d51877cf6f1f28">dy</a>;</div><div class="line">      }</div><div class="line">    }</div><div class="line">    State s = result.back().first;</div><div class="line">    xSucc = s.x + dx * cos(-s.yaw) - dy * sin(-s.yaw);</div><div class="line">    ySucc = s.y + dx * sin(-s.yaw) + dy * cos(-s.yaw);</div><div class="line">    yawSucc = Constants::normalizeHeadingRad(s.yaw + dyaw);</div><div class="line">    result.emplace_back(std::make_pair&lt;&gt;(State(xSucc, ySucc, yawSucc),</div><div class="line">                                         ratio * <a class="code" href="namespaceConstants.html#a73c1390c57181cef88dd82781e2e6505">Constants::dx</a>[0]));</div><div class="line">    <span class="comment">// std::cout &lt;&lt; &quot;Have generate &quot; &lt;&lt; result.size() &lt;&lt; &quot; path segments:\n\t&quot;;</span></div><div class="line">    <span class="comment">// for (auto iter = result.begin(); iter != result.end(); iter++)</span></div><div class="line">    <span class="comment">//   std::cout &lt;&lt; iter-&gt;first &lt;&lt; &quot;:&quot; &lt;&lt; iter-&gt;second &lt;&lt; &quot;-&gt;&quot;;</span></div><div class="line">    <span class="comment">// std::cout &lt;&lt; std::endl;</span></div><div class="line"></div><div class="line">    <span class="keywordflow">return</span> result;</div><div class="line">  }</div><div class="line"></div><div class="line">  <span class="keywordtype">int</span> m_dimx;</div><div class="line">  <span class="keywordtype">int</span> m_dimy;</div><div class="line">  std::unordered_set&lt;State&gt; m_obstacles;</div><div class="line">  std::vector&lt;std::vector&lt;double&gt;&gt; holonomic_cost_map;</div><div class="line">  State m_goal;</div><div class="line">};</div><div class="line"></div><div class="line"><span class="keywordtype">int</span> main() {</div><div class="line">  <span class="comment">// TODO: read map info from yaml</span></div><div class="line">  std::unordered_set&lt;State&gt; obs;</div><div class="line">  obs.insert(State(6.66799, 9.66868, 0));</div><div class="line">  obs.insert(State(6.86099, 6.20241, 0));</div><div class="line">  obs.insert(State(6.2493, 3.45836, 0));</div><div class="line">  obs.insert(State(6.93504, 0.747862, 0));</div><div class="line">  obs.insert(State(6.81566, 4.75936, 0));</div><div class="line">  State goal(13, 10, -M_PI);</div><div class="line">  State start(2, 2, 0);</div><div class="line">  <a class="code" href="classlibMultiRobotPlanning_1_1Environment.html">Environment</a> env(16, 16, obs, goal);</div><div class="line">  <a class="code" href="classlibMultiRobotPlanning_1_1HybridAStar.html">HybridAStar&lt;State, Action, double, Environment&gt;</a> hybridAStar(env);</div><div class="line">  <a class="code" href="structlibMultiRobotPlanning_1_1PlanResult.html">PlanResult&lt;State, Action, double&gt;</a> solution;</div><div class="line">  <a name="_a8"></a><a class="code" href="classTimer.html">Timer</a> timer;</div><div class="line">  <span class="keywordtype">bool</span> searchSuccess = hybridAStar.<a name="a9"></a><a class="code" href="classlibMultiRobotPlanning_1_1HybridAStar.html#aa25fddfe1d2772051e4d596b728450ec">search</a>(start, solution);</div><div class="line">  timer.<a name="a10"></a><a class="code" href="classTimer.html#a63f0eb44b27402196590a03781515dba">stop</a>();</div><div class="line"></div><div class="line">  std::string outputFile = <span class="stringliteral">&quot;output_h.yaml&quot;</span>;</div><div class="line">  std::ofstream out(outputFile);</div><div class="line">  <span class="keywordflow">if</span> (searchSuccess) {</div><div class="line">    std::cout &lt;&lt; <span class="stringliteral">&quot;\033[1m\033[32m Succesfully find a path! \033[0m\n&quot;</span>;</div><div class="line">    std::cout &lt;&lt; <span class="stringliteral">&quot;Solution get &quot;</span> &lt;&lt; solution.<a name="a11"></a><a class="code" href="structlibMultiRobotPlanning_1_1PlanResult.html#ad1bd2882efca968d38165742bae0a511">states</a>.size() &lt;&lt; <span class="stringliteral">&quot; states &quot;</span></div><div class="line">              &lt;&lt; solution.<a name="a12"></a><a class="code" href="structlibMultiRobotPlanning_1_1PlanResult.html#aa868b92b5c742f6c662e69779fbd09a4">actions</a>.size() &lt;&lt; <span class="stringliteral">&quot; moves\n&quot;</span></div><div class="line">              &lt;&lt; <span class="stringliteral">&quot;Runtime: &quot;</span> &lt;&lt; timer.<a name="a13"></a><a class="code" href="classTimer.html#a4415a31ce4efa5af7889d91c6eb9dc17">elapsedSeconds</a>() &lt;&lt; std::endl;</div><div class="line"></div><div class="line">    out &lt;&lt; <span class="stringliteral">&quot;schedule:&quot;</span> &lt;&lt; std::endl;</div><div class="line">    out &lt;&lt; <span class="stringliteral">&quot;  agent1:&quot;</span> &lt;&lt; std::endl;</div><div class="line">    <span class="keywordflow">for</span> (<span class="keywordtype">size_t</span> i = 0; i &lt; solution.<a class="code" href="structlibMultiRobotPlanning_1_1PlanResult.html#ad1bd2882efca968d38165742bae0a511">states</a>.size(); ++i) {</div><div class="line">      out &lt;&lt; <span class="stringliteral">&quot;    - x: &quot;</span> &lt;&lt; solution.<a class="code" href="structlibMultiRobotPlanning_1_1PlanResult.html#ad1bd2882efca968d38165742bae0a511">states</a>[i].first.x &lt;&lt; std::endl</div><div class="line">          &lt;&lt; <span class="stringliteral">&quot;      y: &quot;</span> &lt;&lt; solution.<a class="code" href="structlibMultiRobotPlanning_1_1PlanResult.html#ad1bd2882efca968d38165742bae0a511">states</a>[i].first.y &lt;&lt; std::endl</div><div class="line">          &lt;&lt; <span class="stringliteral">&quot;      yaw: &quot;</span> &lt;&lt; solution.<a class="code" href="structlibMultiRobotPlanning_1_1PlanResult.html#ad1bd2882efca968d38165742bae0a511">states</a>[i].first.yaw &lt;&lt; std::endl</div><div class="line">          &lt;&lt; <span class="stringliteral">&quot;      t: &quot;</span> &lt;&lt; i &lt;&lt; std::endl;</div><div class="line">    }</div><div class="line">    <span class="comment">// for (auto iter = solution.states.begin(); iter != solution.states.end();</span></div><div class="line">    <span class="comment">//      iter++)</span></div><div class="line">    <span class="comment">//   std::cout &lt;&lt; iter-&gt;first &lt;&lt; &quot;:&quot; &lt;&lt; iter-&gt;second &lt;&lt; &quot;-&gt;&quot;;</span></div><div class="line">    <span class="comment">// std::cout &lt;&lt; std::endl;</span></div><div class="line">    <span class="comment">// for (auto iter = solution.actions.begin(); iter !=</span></div><div class="line">    <span class="comment">// solution.actions.end();</span></div><div class="line">    <span class="comment">//      iter++)</span></div><div class="line">    <span class="comment">//   std::cout &lt;&lt; iter-&gt;first &lt;&lt; &quot;:&quot; &lt;&lt; iter-&gt;second &lt;&lt; &quot;-&gt;&quot;;</span></div><div class="line">    <span class="comment">// std::cout &lt;&lt; std::endl;</span></div><div class="line">    std::cout &lt;&lt; <span class="stringliteral">&quot;Solution: gscore/cost:&quot;</span> &lt;&lt; solution.<a name="a14"></a><a class="code" href="structlibMultiRobotPlanning_1_1PlanResult.html#ab340aeae4fdabbd0345403e9fa609d9a">cost</a></div><div class="line">              &lt;&lt; <span class="stringliteral">&quot;\t fmin:&quot;</span> &lt;&lt; solution.<a name="a15"></a><a class="code" href="structlibMultiRobotPlanning_1_1PlanResult.html#aefdd2f68b906f207ded16bfe46e0f7b0">fmin</a> &lt;&lt; <span class="stringliteral">&quot;\n\rDiscover &quot;</span> &lt;&lt; env.Dcount</div><div class="line">              &lt;&lt; <span class="stringliteral">&quot; Nodes and Expand &quot;</span> &lt;&lt; env.Ecount &lt;&lt; <span class="stringliteral">&quot; Nodes.&quot;</span> &lt;&lt; std::endl;</div><div class="line">  } <span class="keywordflow">else</span> {</div><div class="line">    std::cout &lt;&lt; <span class="stringliteral">&quot;\033[1m\033[31m Fail to find a path \033[0m\n&quot;</span>;</div><div class="line">  }</div><div class="line">}</div></div><!-- fragment --> </div><!-- contents -->
<!-- start footer part -->
<hr class="footer"/><address class="footer"><small>
Generated by &#160;<a href="http://www.doxygen.org/index.html">
<img class="footer" src="doxygen.png" alt="doxygen"/>
</a> 1.8.11
</small></address>
</body>
</html>
